package soundsourceloc;

import gnu.io.CommPortIdentifier;
import gnu.io.PortInUseException;
import gnu.io.SerialPort;
import gnu.io.UnsupportedCommOperationException;

import java.awt.BasicStroke;
import java.awt.BorderLayout;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.HeadlessException;
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStream;
import java.io.InputStreamReader;
import java.io.OutputStream;
import java.util.Enumeration;
import java.util.logging.Level;
import java.util.logging.Logger;

import javax.swing.JFrame;
import javax.swing.JPanel;

/**
 * This class draws the direction of the source of the sound it hears.
 * 
 * It gets its from a C++ program (see companion project 'sound-source-loc')
 * 
 * The direction of the sound is then drawn on screen (if a screen is
 * available), and used to control a robot head pan servo (through a serial
 * command sent to an Arduino board).
 * 
 * @author Frederic Pesquet (fpesquet at gmail dot com)
 */
public class SoundSourceCanon {
	private static final long serialVersionUID = 1L;

	protected static final Logger LOG = Logger.getLogger(SoundSourceCanon.class
			.getName());

	/** last time we moved (absolute time in system ms) */
	private long _lastTimeMoveDone = System.currentTimeMillis();

	/**
	 * min time to move again after first move, in ms
	 */
	static private final long _minTimeBetweenMoves = 1000;

	/**
	 * Name of the serial port used to talk to the Arduino controller which
	 * handles the robot head pan servo
	 */
	static private final String _arduinoSerialPortName = "/dev/ttyUSB0";

	/** serial port to arduino board */
	private SerialPort _arduinoSerialPort;

	/** serial stream */
	private OutputStream _arduinoSerialPortOutput;

	/** the panel that draw the last sound localization as an arc */
	private SoundLocDraw _soundLocDraw = null;

	public SoundSourceCanon() throws Exception {
		setupDisplay();
		connectToArduinoCtrl();
		Thread.sleep(500);
		setServoTo('P', 0);
		setMotorTo(200);
		Runtime.getRuntime().addShutdownHook(new Thread() {
			@Override
			public void run() {
				disconnectFromArduinoCtrl();
			}
		});
	}

	private void setupDisplay() {
		try {
			JFrame frame = new JFrame("Sound Source Localization");
			_soundLocDraw = new SoundLocDraw();
			frame.getContentPane().add(_soundLocDraw, BorderLayout.CENTER);
			frame.setSize(800, 400);
			frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
			frame.setLocationRelativeTo(null);
			frame.setVisible(true);
		} catch (HeadlessException ex) {
			// no screen. Don't display anything
			LOG.warning("No display, will not draw location");
		}
	}

	/**
	 * Main loop: launch C++ listener, get its output, draw, and loop
	 * 
	 * @throws IOException
	 */
	public void run() throws IOException {
		ProcessBuilder pb = new ProcessBuilder("ssh", "pi@192.168.1.121",
				"/home/pi/sound-source-loc/src/sound-source-loc");
		pb = pb.redirectErrorStream(true);
		Process p = pb.start();
		InputStream is = p.getInputStream();
		InputStreamReader isr = new InputStreamReader(is);
		BufferedReader br = new BufferedReader(isr);
		String line;
		while ((line = br.readLine()) != null) {
			int sep = line.indexOf(';');
			try {
				float angle = Float.parseFloat(line.substring(0, sep));
				float relativePower = Float.parseFloat(line.substring(sep + 1));
				// System.out.println("received sound loc: "+line);

				// as we move the servo head and it makes sound, this
				// disturb the localization process and results in oscillations
				// and bad detections after the first one.
				// => to avoid this, stop reacting for some time once we have
				// decided for a direction
				if (System.currentTimeMillis() > _lastTimeMoveDone
						+ _minTimeBetweenMoves) {
					_lastTimeMoveDone = System.currentTimeMillis();
					if (_soundLocDraw != null) {
						_soundLocDraw.setSound(angle, relativePower);
					}
					setServoTo('P', angle);
					if (relativePower > 3.0) {
						Thread.sleep(500);
						fire();
					}
				}
			} catch (Exception ex) {
				LOG
						.warning("unreadable from sound loc proc, skipping: "
								+ line);
			}
		}
	}

	@SuppressWarnings("unchecked")
	private void connectToArduinoCtrl() throws IOException, PortInUseException,
			UnsupportedCommOperationException {
		Enumeration<CommPortIdentifier> portList = CommPortIdentifier
				.getPortIdentifiers();
		while (portList.hasMoreElements()) {
			CommPortIdentifier portId = portList.nextElement();
			if (portId.getName().equals(_arduinoSerialPortName)) {
				LOG.info("open_port:" + portId.getName());
				_arduinoSerialPort = (SerialPort) portId.open("Arduino Serial",
						2000);
				// _serialPort.setFlowControlMode(SerialPort.FLOWCONTROL_NONE);
				// _arduinoSerialPortInput =
				// _arduinoSerialPort.getInputStream();
				_arduinoSerialPortOutput = _arduinoSerialPort.getOutputStream();
				_arduinoSerialPort.setSerialPortParams(9600, 8,
						SerialPort.STOPBITS_1, SerialPort.PARITY_NONE);
			}
		}
		if (_arduinoSerialPort == null) {
			throw new IOException("Can't connect to Arduino");
		}
	}

	private void disconnectFromArduinoCtrl() {
		LOG.log(Level.INFO, "shutdown motor");
		setMotorTo(0);
		try {
			Thread.sleep(1000);
		} catch (InterruptedException e1) {
		}
		try {
			if (_arduinoSerialPort != null) {
				LOG.log(Level.INFO, "closing serial port");
				_arduinoSerialPort.close();
			}
		} catch (Exception e) {
			LOG.log(Level.WARNING, "can't close serial port", e);
		}
		_arduinoSerialPort = null;
	}

	/**
	 * Set desired position for given servo
	 * 
	 * @param servo
	 *            one of 'F' (fire), 'P' (pan), 'T' (tilt)
	 * @param anglerad
	 *            desired angle, in radians. 0 = middle.
	 */
	private void setServoTo(char servo, double anglerad) {
		try {
			int servoAngle = (int) Math.toDegrees(anglerad) + 90;
			if (servoAngle < 0)
				servoAngle = 0;
			if (servoAngle > 180)
				servoAngle = 180;
			LOG.log(Level.INFO, "set angle for " + servo + " to " + servoAngle);
			_arduinoSerialPortOutput.write('S');
			_arduinoSerialPortOutput.write(servo);
			_arduinoSerialPortOutput.write(servoAngle);
			_arduinoSerialPortOutput.flush();
		} catch (Exception e) {
			LOG.log(Level.WARNING, "Error occurred writing to arduino", e);
		}
	}

	/**
	 * Set desired speed for motor (-127..127)
	 */
	private void setMotorTo(int speed) {
		try {
			LOG.log(Level.INFO, "set motor speed to " + speed);
			_arduinoSerialPortOutput.write('S');
			_arduinoSerialPortOutput.write('M');
			_arduinoSerialPortOutput.write(speed);
			_arduinoSerialPortOutput.flush();
		} catch (Exception e) {
			LOG.log(Level.WARNING, "Error occurred writing to arduino", e);
		}
	}

	/**
	 * Fire!
	 */
	private void fire() {
		setServoTo('F', Math.toRadians(9));
		try {
			Thread.sleep(500);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		setServoTo('F', Math.toRadians(39));
	}

	/**
	 * A simple panel that draws the sound source localization angle, with a
	 * thickness depending on the sound level.
	 */
	@SuppressWarnings("serial")
	static private class SoundLocDraw extends JPanel {
		// sound angle, between -PI/2...+PI/2
		private float _angle;

		// relative power with respect to mean power (1.0=mean power)
		private float _relativePower;

		public void setSound(float angle, float relativePower) {
			_angle = angle;
			_relativePower = relativePower;
			repaint();
		}

		@Override
		protected void paintComponent(Graphics g) {
			super.paintComponent(g);
			Graphics2D g2d = (Graphics2D) g;
			Dimension d = getSize();
			int radius = Math.min(d.height, d.width / 2);
			int cx = d.width / 2;
			int cy = 0;
			int tx = cx + (int) (Math.cos(_angle + Math.PI / 2) * radius);
			int ty = cy + (int) (Math.sin(_angle + Math.PI / 2) * radius);
			g2d.drawOval(cx - radius, cy - radius, radius * 2, radius * 2);
			// use larger strokes for louder sounds:
			g2d.setStroke(new BasicStroke(1 + (int) ((Math.max(_relativePower,
					1) - 1.0) * 10)));
			g2d.drawLine(cx, cy, tx, ty);
		}
	}

	/**
	 * Entry point: create the frame, and start listening to sound until closed.
	 */
	public static void main(String[] args) throws Exception {
		SoundSourceCanon snd = new SoundSourceCanon();
		snd.run();
	}
}
